#include "cartographer/mapping/internal/constraints/constraint_builder_2d.h"

#include <cmath>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <memory>
#include <sstream>
#include <string>
#include <tuple>
#include <utility>

#include "Eigen/Eigenvalues"
#include "absl/memory/memory.h"
#include "cartographer/common/math.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.pb.h"
#include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_2d.pb.h"
#include "cartographer/metrics/counter.h"
#include "cartographer/metrics/gauge.h"
#include "cartographer/metrics/histogram.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"

namespace cartographer
{
    namespace mapping
    {
        namespace constraints
        {

            static auto *kConstraintsSearchedMetric = metrics::Counter::Null();
            static auto *kConstraintsFoundMetric = metrics::Counter::Null();
            static auto *kGlobalConstraintsSearchedMetric = metrics::Counter::Null();
            static auto *kGlobalConstraintsFoundMetric = metrics::Counter::Null();
            static auto *kQueueLengthMetric = metrics::Gauge::Null();
            static auto *kConstraintScoresMetric = metrics::Histogram::Null();
            static auto *kGlobalConstraintScoresMetric = metrics::Histogram::Null();
            static auto *kNumSubmapScanMatchersMetric = metrics::Gauge::Null();

            transform::Rigid2d ComputeSubmapPose(const Submap2D &submap)
            {
                return transform::Project2D(submap.local_pose());
            }

            /// @brief 该类就是检测回环和新增回环约束，其实就是暴力匹配去检测有没有回环，有的话就把节点和找到的子图加一个约束
            /// @param options
            /// @param thread_pool
            ConstraintBuilder2D::ConstraintBuilder2D(
                const constraints::proto::ConstraintBuilderOptions &options,
                common::ThreadPoolInterface *const thread_pool)
                : options_(options),
                  thread_pool_(thread_pool),
                  finish_node_task_(absl::make_unique<common::Task>()),
                  when_done_task_(absl::make_unique<common::Task>()),
                  ceres_scan_matcher_(options.ceres_scan_matcher_options())
            {
            }

            ConstraintBuilder2D::~ConstraintBuilder2D()
            {
                absl::MutexLock locker(&mutex_);
                CHECK_EQ(finish_node_task_->GetState(), common::Task::NEW);
                CHECK_EQ(when_done_task_->GetState(), common::Task::NEW);
                CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
                CHECK_EQ(num_started_nodes_, num_finished_nodes_);
                CHECK(when_done_ == nullptr);
            }

            /// @brief 计算节点和子图约束,需初始位姿,需在同一个轨迹.该函数逻辑是创建约束计算任务放入线程池等待调度，实际的约束
            /// 计算函数是ComputeConstraint
            /// @param submap_id
            /// @param submap
            /// @param node_id
            /// @param constant_data
            /// @param initial_relative_pose
            void ConstraintBuilder2D::MaybeAddConstraint(
                const SubmapId &submap_id,
                const Submap2D *const submap,
                const NodeId &node_id,
                const TrajectoryNode::Data *const constant_data,
                const transform::Rigid2d &initial_relative_pose)
            {
                if (initial_relative_pose.translation().norm() > options_.max_constraint_distance())
                {
                    // 超过搜索框的大小.一定程度上降低约束数量，减少全局图优化计算量;如两者相差太远，很可能受到累积误差影响导致添加错误的约束
                    return;
                }
                if (!per_submap_sampler_
                         .emplace(std::piecewise_construct, std::forward_as_tuple(submap_id),
                                  std::forward_as_tuple(options_.sampling_ratio()))
                         .first->second.Pulse())
                {
                    return;
                }

                absl::MutexLock locker(&mutex_);
                if (when_done_)
                {
                    // 上一个闭环检测迭代还没有完全结束
                    LOG(WARNING) << "MaybeAddConstraint was called while WhenDone was scheduled.";
                }
                constraints_.emplace_back();
                kQueueLengthMetric->Set(constraints_.size());
                auto *const constraint = &constraints_.back();
                // 为每一个子图分配一个匹配器fast scan matcher. matcher构建是一个线程任务，在线程池中执行构建，不在下面这个函数中构建
                const auto *scan_matcher = DispatchScanMatcherConstruction(submap_id, submap->grid());
                auto constraint_task = absl::make_unique<common::Task>();
                // 创建约束计算任务
                constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) { ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */
                                                                                              constant_data, initial_relative_pose, *scan_matcher,
                                                                                              constraint); });
                constraint_task->AddDependency(scan_matcher->creation_task_handle);
                // 将约束计算任务放入线程池等待调度.
                auto constraint_task_handle = thread_pool_->Schedule(std::move(constraint_task));
                // 约束任务计算完成执行计数任务
                finish_node_task_->AddDependency(constraint_task_handle);
            }

            /// @brief 计算节点和子图间的约束；全局匹配,不需初始位姿.因此调用函数是ComputeConstraint位姿设置为Identity，不要求在同一轨迹.
            /// 该函数如上也是创建约束计算任务放入线程池等待调度
            /// @param submap_id
            /// @param submap
            /// @param node_id
            /// @param constant_data
            void ConstraintBuilder2D::MaybeAddGlobalConstraint(
                const SubmapId &submap_id,
                const Submap2D *const submap,
                const NodeId &node_id,
                const TrajectoryNode::Data *const constant_data)
            {
                absl::MutexLock locker(&mutex_);
                if (when_done_)
                {
                    LOG(WARNING) << "MaybeAddGlobalConstraint was called while WhenDone was scheduled.";
                }
                constraints_.emplace_back();
                kQueueLengthMetric->Set(constraints_.size());
                auto *const constraint = &constraints_.back();
                // 得到子图对应的scan-matcher.scan_matcher在由线程池调度构建
                const auto *scan_matcher = DispatchScanMatcherConstruction(submap_id, submap->grid());
                auto constraint_task = absl::make_unique<common::Task>();
                // 约束计算任务.
                constraint_task->SetWorkItem([=]() LOCKS_EXCLUDED(mutex_) { ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */
                                                                                              constant_data, transform::Rigid2d::Identity(),
                                                                                              *scan_matcher, constraint); });
                // 约束计算任务要在scan_matcher构建之后执行
                constraint_task->AddDependency(scan_matcher->creation_task_handle);
                // 进入线程池等待调度.
                auto constraint_task_handle = thread_pool_->Schedule(std::move(constraint_task));
                // finish_node_task_用于计数，待约束计算结束后执行计数任务
                finish_node_task_->AddDependency(constraint_task_handle);
            }

            void ConstraintBuilder2D::NotifyEndOfNode()
            {
                absl::MutexLock locker(&mutex_);
                CHECK(finish_node_task_ != nullptr);
                auto work = [this] {
                    absl::MutexLock locker(&mutex_);
                    ++num_finished_nodes_;
                };
                finish_node_task_->SetWorkItem(work);
                auto finish_node_task_handle = thread_pool_->Schedule(std::move(finish_node_task_));
                finish_node_task_ = absl::make_unique<common::Task>();
                when_done_task_->AddDependency(finish_node_task_handle);
                ++num_started_nodes_;
            }

            void ConstraintBuilder2D::WhenDone(const std::function<void(const ConstraintBuilder2D::Result &)> &callback)
            {
                absl::MutexLock locker(&mutex_);
                CHECK(when_done_ == nullptr);
                // TODO(gaschler): Consider using just std::function, it can also be empty.
                when_done_ = absl::make_unique<std::function<void(const Result &)>>(callback);
                CHECK(when_done_task_ != nullptr);
                when_done_task_->SetWorkItem([this] { RunWhenDoneCallback(); });
                thread_pool_->Schedule(std::move(when_done_task_));
                when_done_task_ = absl::make_unique<common::Task>();
            }

            // 为每一个子图submap_id分配一个scan-matcher.如果该子图已经有scan-matcher则直接返回,如果该子图没有scan-matcher.则为该子图分配一个。
            // 涉及到金字塔的计算,因此需要通过线程的方法来进行计算.
            const ConstraintBuilder2D::SubmapScanMatcher *
            ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId &submap_id, const Grid2D *const grid)
            {
                CHECK(grid);
                if (submap_scan_matchers_.count(submap_id) != 0)
                {
                    return &submap_scan_matchers_.at(submap_id);
                }
                auto &submap_scan_matcher = submap_scan_matchers_[submap_id];
                kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size());
                submap_scan_matcher.grid = grid;
                // 快速CSM
                auto &scan_matcher_options = options_.fast_correlative_scan_matcher_options();
                auto scan_matcher_task = absl::make_unique<common::Task>();
                scan_matcher_task->SetWorkItem(
                    [&submap_scan_matcher, &scan_matcher_options]() {
                        submap_scan_matcher.fast_correlative_scan_matcher = absl::make_unique<scan_matching::FastCorrelativeScanMatcher2D>(
                            *submap_scan_matcher.grid, scan_matcher_options);
                    });
                submap_scan_matcher.creation_task_handle = thread_pool_->Schedule(std::move(scan_matcher_task));
                return &submap_scan_matchers_.at(submap_id);
            }

            /// @brief 该类的核心函数；计算节点和子图之的位姿关系，用fast csm来进行匹配, 然后用csm来进行优化.
            /// @param submap_id
            /// @param submap
            /// @param node_id
            /// @param match_full_submap
            /// @param constant_data
            /// @param initial_relative_pose
            /// @param submap_scan_matcher
            /// @param constraint
            void ConstraintBuilder2D::ComputeConstraint(
                const SubmapId &submap_id,
                const Submap2D *const submap,
                const NodeId &node_id,
                bool match_full_submap,
                const TrajectoryNode::Data *const constant_data,
                const transform::Rigid2d &initial_relative_pose,
                const SubmapScanMatcher &submap_scan_matcher,
                std::unique_ptr<ConstraintBuilder2D::Constraint> *constraint)
            {
                CHECK(submap_scan_matcher.fast_correlative_scan_matcher);
                // 匹配的初始位姿.
                const transform::Rigid2d initial_pose = ComputeSubmapPose(*submap) * initial_relative_pose;

                // The 'constraint_transform' (submap i <- node j) is computed from:
                // - a 'filtered_gravity_aligned_point_cloud' in node j,
                // - the initial guess 'initial_pose' for (map <- node j),
                // - the result 'pose_estimate' of Match() (map <- node j).
                // - the ComputeSubmapPose() (map <- submap i)
                float score = 0.;
                // 初始位姿
                transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();

                // Compute 'pose_estimate' in three stages:
                // 1. Fast estimate using the fast correlative scan matcher.
                // 2. Prune if the score is too low.
                // 3. Refine.
                // make_full_submap就是无初始值，全局约束
                if (match_full_submap)
                {
                    kGlobalConstraintsSearchedMetric->Increment();
                    // 进行无初始位姿fast csm匹配.即全局匹配.
                    if (submap_scan_matcher.fast_correlative_scan_matcher->MatchFullSubmap(
                            constant_data->filtered_gravity_aligned_point_cloud,
                            options_.global_localization_min_score(), &score, &pose_estimate))
                    {
                        CHECK_GT(score, options_.global_localization_min_score());
                        CHECK_GE(node_id.trajectory_id, 0);
                        CHECK_GE(submap_id.trajectory_id, 0);
                        kGlobalConstraintsFoundMetric->Increment();
                        kGlobalConstraintScoresMetric->Observe(score);
                    }
                    else
                    {
                        return;
                    }
                }
                else
                {
                    kConstraintsSearchedMetric->Increment();
                    // 进行有初始位姿的fast csm匹配.
                    if (submap_scan_matcher.fast_correlative_scan_matcher->Match(
                            initial_pose, constant_data->filtered_gravity_aligned_point_cloud,
                            options_.min_score(), &score, &pose_estimate))
                    {
                        // We've reported a successful local match.
                        CHECK_GT(score, options_.min_score());
                        kConstraintsFoundMetric->Increment();
                        kConstraintScoresMetric->Observe(score);
                    }
                    else
                    {
                        return;
                    }
                }
                {
                    absl::MutexLock locker(&mutex_);
                    score_histogram_.Add(score);
                }

                // Use the CSM estimate as both the initial and previous pose. This has the
                // effect that, in the absence of better information, we prefer the original
                // CSM estimate.
                // 进行scan-match优化.这部分的内容和前端建图的逻辑一样
                ceres::Solver::Summary unused_summary;
                ceres_scan_matcher_.Match(pose_estimate.translation(), pose_estimate,
                                          constant_data->filtered_gravity_aligned_point_cloud,
                                          *submap_scan_matcher.grid, &pose_estimate,
                                          &unused_summary);
                // 计算得到的约束.
                const transform::Rigid2d constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate;
                // 根据计算的位姿,重新设置约束的值. 可以认为增加了新的约束.
                constraint->reset(new Constraint{submap_id,
                                                 node_id,
                                                 {transform::Embed3D(constraint_transform),
                                                  options_.loop_closure_translation_weight(),
                                                  options_.loop_closure_rotation_weight()},
                                                 Constraint::INTER_SUBMAP});

                if (options_.log_matches())
                {
                    std::ostringstream info;
                    info << "Node " << node_id << " with "
                         << constant_data->filtered_gravity_aligned_point_cloud.size()
                         << " points on submap " << submap_id << std::fixed;
                    if (match_full_submap)
                    {
                        info << " matches";
                    }
                    else
                    {
                        const transform::Rigid2d difference = initial_pose.inverse() * pose_estimate;
                        info << " differs by translation " << std::setprecision(2)
                             << difference.translation().norm() << " rotation "
                             << std::setprecision(3) << std::abs(difference.normalized_angle());
                    }
                    info << " with score " << std::setprecision(1) << 100. * score << "%.";
                    LOG(INFO) << info.str();
                }
            }

            void ConstraintBuilder2D::RunWhenDoneCallback()
            {
                Result result;
                std::unique_ptr<std::function<void(const Result &)>> callback;
                {
                    absl::MutexLock locker(&mutex_);
                    CHECK(when_done_ != nullptr);
                    for (const std::unique_ptr<Constraint> &constraint : constraints_)
                    {
                        if (constraint == nullptr)
                            continue;
                        result.push_back(*constraint);
                    }
                    if (options_.log_matches())
                    {
                        LOG(INFO) << constraints_.size() << " computations resulted in "
                                  << result.size() << " additional constraints.";
                        LOG(INFO) << "Score histogram:\n"
                                  << score_histogram_.ToString(10);
                    }
                    constraints_.clear();
                    callback = std::move(when_done_);
                    when_done_.reset();
                    kQueueLengthMetric->Set(constraints_.size());
                }
                (*callback)(result);
            }

            int ConstraintBuilder2D::GetNumFinishedNodes()
            {
                absl::MutexLock locker(&mutex_);
                return num_finished_nodes_;
            }

            void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId &submap_id)
            {
                absl::MutexLock locker(&mutex_);
                if (when_done_)
                {
                    LOG(WARNING)
                        << "DeleteScanMatcher was called while WhenDone was scheduled.";
                }
                submap_scan_matchers_.erase(submap_id);
                per_submap_sampler_.erase(submap_id);
                kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size());
            }

            void ConstraintBuilder2D::RegisterMetrics(metrics::FamilyFactory *factory)
            {
                auto *counts = factory->NewCounterFamily(
                    "mapping_constraints_constraint_builder_2d_constraints",
                    "Constraints computed");
                kConstraintsSearchedMetric = counts->Add({{"search_region", "local"}, {"matcher", "searched"}});
                kConstraintsFoundMetric = counts->Add({{"search_region", "local"}, {"matcher", "found"}});
                kGlobalConstraintsSearchedMetric = counts->Add({{"search_region", "global"}, {"matcher", "searched"}});
                kGlobalConstraintsFoundMetric = counts->Add({{"search_region", "global"}, {"matcher", "found"}});
                auto *queue_length = factory->NewGaugeFamily(
                    "mapping_constraints_constraint_builder_2d_queue_length", "Queue length");
                kQueueLengthMetric = queue_length->Add({});
                auto boundaries = metrics::Histogram::FixedWidth(0.05, 20);
                auto *scores = factory->NewHistogramFamily(
                    "mapping_constraints_constraint_builder_2d_scores",
                    "Constraint scores built", boundaries);
                kConstraintScoresMetric = scores->Add({{"search_region", "local"}});
                kGlobalConstraintScoresMetric = scores->Add({{"search_region", "global"}});
                auto *num_matchers = factory->NewGaugeFamily(
                    "mapping_constraints_constraint_builder_2d_num_submap_scan_matchers",
                    "Current number of constructed submap scan matchers");
                kNumSubmapScanMatchersMetric = num_matchers->Add({});
            }

        } // namespace constraints
    }     // namespace mapping
} // namespace cartographer
